﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NKH.MindSqualls;
using System.Threading;
namespace schrodinger
{
    class HTMotor : DigitalSensor
    {
        public HTMotor()
            : base()
        { }

        public struct position
        {
            public float left, right;

            public position(float l, float r)
            {
                left = l;
                right = r;
            }
        }

        #region methods

        /// <summary>
        /// <para>Zeros out the encoder positions.</para>
        /// </summary>
        /// <returns>... nothing ...</returns>
        public void resetPosition()
        {
            byte[] request = new byte[] { 0x02, M1_MODE }; //read mode
            byte[] reply = base.SendN(request, 4);
            byte d = reply[0];
            byte [] command = new byte [] {0x02, (byte)(d & MODE_MSK | MODE_RST | MODE_NTO | MODE_REV) }; //mode reverse reminder
            reply = SendN(command, 4);
            request = new byte[] { 0x02, M2_MODE }; //read mode
            reply = base.SendN(request, 4);
            d = reply[0];
            command = new byte [] {0x02, (byte)(d & MODE_MSK | MODE_RST | MODE_NTO | MODE_REV) };
            reply = SendN(command, 4);
        }

        public position getPosition()
        {
            position ticks;
            byte[] request = new byte[] { 0x02, M1_ENC }; //read left encoder
            byte[] reply = base.SendN(request, 4);
            ticks.left = BitConverter.ToSingle(reply, 0);
            request = new byte[] { 0x02, M2_ENC }; //read right encoder
            reply = base.SendN(request, 4);
            ticks.right = BitConverter.ToSingle(reply, 0);
            return ticks;
        }

        public double getVoltage()
        {
            byte? reply = ReadByteFromAddress(BATT_V);
            return (double) reply * .08;
        }

        public int i2cRead(int address, int txByte)
        {
            int result=0;
            byte[] request = new byte[] { 0x02, (byte)address };
            byte[] reply = base.SendN(request,(byte)txByte);
            result = BitConverter.ToInt32(reply,0);
            return result;
        }

        public void i2cWrite(byte [] command, int txByte)
        {
            base.SendN(command, (byte)txByte);
        }

        public void setModeSpeed()
        {
            byte? motor1mode = ReadByteFromAddress(M1_MODE), motor2mode = ReadByteFromAddress(M2_MODE); //get motor modes
            byte commandSev1 = (byte)(((int)motor1mode & (int)MODE_MSK) | (int)MODE_SPD | (int)MODE_NTO | (int)MODE_REV), commandSev2 = (byte)(((int)motor1mode & (int)MODE_MSK) | (int)MODE_SPD | (int)MODE_NTO);
            CommandToAddress(0x44, commandSev1);
            CommandToAddress(0x47, commandSev2);
        }

        public void setModeServo()
        {
            byte? motor1mode = ReadByteFromAddress(M1_MODE),motor2mode = ReadByteFromAddress(M2_MODE); //get motor modes
            byte commandSev1 = (byte)(((int)motor1mode & (int)MODE_MSK) | (int)MODE_SRV | (int)MODE_NTO | (int)MODE_REV), commandSev2 = (byte)(((int)motor1mode & (int)MODE_MSK) | (int)MODE_SRV | (int)MODE_NTO );
            CommandToAddress(0x44, commandSev1);
            CommandToAddress(0x47, commandSev2);
        }

        public int speed2pwr(double speed) 
        {
	        return (int)(speed*TIC_PER_MM)/TIC_PER_SEC_PER_PWR;
        }

        public int dist2tic(double mm) 
        {
	        return (int)(mm*TIC_PER_MM); // mm * tick/mm = tick
        }

        public void setSpeed(double spdLeft, double spdRight)
        {
            
            position start, current;
            int attempt; 
           
            int waitDelay; 
            double slowestNonZeroSpeed;
            
            double capFactor = Math.Min(1.0,	TOP_SPEED / Math.Max(Math.Abs(spdLeft),Math.Abs(spdRight)));
            
            spdLeft*= capFactor;
            spdRight*= capFactor;
            slowestNonZeroSpeed = Math.Min(Math.Abs(spdLeft), Math.Abs(spdRight));
            if (slowestNonZeroSpeed == 0)
                slowestNonZeroSpeed = Math.Max(Math.Abs(spdLeft), Math.Abs(spdRight));
            if (slowestNonZeroSpeed == 0) return;
                start = getPosition();

            CommandToAddress(M1_PWR, (byte)speed2pwr(spdLeft));
            CommandToAddress(M2_PWR, (byte)speed2pwr(spdRight));

          /*  for (attempt = 0; attempt < MOTOR_COMMAND_RETRIES; attempt++) 
            {
                
                waitDelay = 1000000 * MOTOR_CHECK_DISTANCE_MM / (int) slowestNonZeroSpeed;
                /* If the delay is more than a second (we're really slow), do a
                regular sleep for the integer seconds. 
                while (waitDelay > 1000000) 
                {
                    waitDelay = 1000;
                    Thread.Sleep(1);
                }
                Thread.Sleep(waitDelay); // And sleep out the remainder.
                current = getPosition();
                if (spdLeft != 0 && (Math.Abs(current.left - start.left) < MOTOR_ENCODER_CHECK_THRESHOLD))
                {
                   // command = new byte[] { 0x02, M1_PWR };
                   // i2cWrite(command, speed2pwr(spdLeft)); // Try again left.
                    CommandToAddress(M1_PWR, (byte)speed2pwr(spdLeft));
                }
                if (spdRight != 0 && (Math.Abs(current.right - start.right) < MOTOR_ENCODER_CHECK_THRESHOLD))
                {
                   // command = new byte[] { 0x02, M2_PWR };
                  //  i2cWrite(command, speed2pwr(spdRight)); // Try again right.
                    CommandToAddress(M2_PWR, (byte)speed2pwr(spdRight));
                }
            }*/
        }

        public void setTargetTicks(int trgLeft, int trgRight)
        {
            position here = getPosition();
            int ticLeft, ticRight;
            int returnedTicLeft, returnedTicRight;
            int attempt;
            ticLeft = trgLeft + (int)here.left;
            ticRight = trgRight + (int)here.right;
            for (attempt = 0; attempt < MOTOR_COMMAND_RETRIES; attempt++) 
            {
                for (int i=0; i<4; i++) 
                { /* Target is a 32bit
                    value stored in 4
                    sequential bytes. */
                    byte [] command = new byte[] { 0x02, (byte)(M1_TRG + i) };
                    i2cWrite(command, ticLeft>>(8*(3*i))& 0xFF );
                    command = new byte[] { 0x02, (byte)(M2_TRG + i) };
                    i2cWrite(command, ticRight >> (8 * (3 * i)) & 0xFF);
                }

                returnedTicLeft = i2cRead(M1_TRG,4);

                returnedTicRight = i2cRead(M2_TRG,4);

                if (returnedTicLeft == ticLeft && returnedTicRight == ticRight) 
                    break;
            }
        }

        public void stop() 
        {
            setTargetTicks(0,0);
            resetPosition();
            setSpeed(0,0);
        }

        public void driveTo(double dist, double speed)
        {
	       
             setModeServo();
	         setTarget(dist, dist);
	         setSpeed(speed, speed);
        }

        public void drive(double speed) 
        {
            byte? motor1mode = ReadByteFromAddress(M1_MODE);
            CommandToAddress(0x44,0x10 );
            CommandToAddress(0x47, 0x11);
            CommandToAddress(0x45, 30);
            CommandToAddress(0x46, 30);
            //setModeSpeed();
	        //setSpeed(speed, speed);

        }

        public int busy() 
        {
	        
            return (i2cRead(M1_MODE, 1) & MODE_BSY) ;
        }
       
        //public position getTarget() 
        //{
        //    position ticks;
        //    ticks.left = i2cRead(M1_TRG,4)/TIC_PER_MM;
        //    ticks.right = i2cRead(M2_TRG,4)/TIC_PER_MM;
        //    return ticks;
        //}

        public void setTarget(double trgLeft, double trgRight)
        {
            setTargetTicks(dist2tic(trgLeft),dist2tic(trgRight));
        }
        

        #endregion

        #region I2C protocol values

        public readonly byte SETINPUTMODE  = 0x05;
        public readonly byte LOWSPEED9V = 0x0B;
        public readonly byte RAWMODE = 0x00;
        public readonly byte M1_MODE = 0x44;
        public readonly byte M1_PWR = 0x45;
        public readonly byte M1_ENC = 0x4C;
        public readonly byte M1_TRG = 0x40;
        public readonly byte M2_MODE = 0x47;
        public readonly byte M2_PWR = 0x46;
        public readonly byte M2_ENC = 0x50;
        public readonly byte M2_TRG = 0x48;
        public readonly byte MODE_MSK = 0xFC; // mask for the mode select bits
        public readonly byte MODE_PWM = 0x0;
        public readonly byte MODE_SPD = 0x1;
        public readonly byte MODE_SRV = 0x2;
        public readonly byte MODE_REV = 0x8;
        public readonly byte MODE_RST = 0x3;
        public readonly byte MODE_BSY = 0x80;
        public readonly byte MODE_NTO = 0x0; // no disable timeout
        public readonly byte BATT_V = 0x54; //battery
        public readonly int MOTOR_W = 141; // 1/2 the width of the wheelbase, in mm
        public readonly int MOTOR_L = 127; // 1/2 the length of the wheelbase, in mm
        public readonly double TIC_PER_MM = 11.5;//8.7 // mm of travel/encoder tick
        public readonly int TIC_PER_SEC_PER_PWR = 40; // ticks/second/pwr increment
        
        public double TOP_SPEED = 100 * 40 / 11.5; // highest representable speed in mm/s
        public readonly double PI = 3.1459;
        /* Here are the defines for the motor speed setting retries. */
        // Try to get it moving 5 times before giving up..
        public readonly int MOTOR_COMMAND_RETRIES = 5;
        // How far in millimeters should we wait for a motor to move before
        // checking up on it
        public readonly int MOTOR_CHECK_DISTANCE_MM = 5;
        public double MOTOR_ENCODER_CHECK_THRESHOLD = (11.5 * 5 / 2);
        // motor 2 is on the left
        // speeds are in mm/s, distances are in mm
        // angles are in degrees
        #endregion
        

    }
}
